#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include "Vector3.h"

/* compile with -lpthread and -llo */

#include <pthread.h>
#include "lo/lo.h"

#define UPDATE_PATH "/pfs/listener/0001/updateAxes"
#define INFINITY_COMMAND "/infinity/target"
#define PLC_NUMBER 7
#define LISTEN_PORT "25669"
#define LOCAL_PORT "25670"
#define LOOPS 2000
#define NUM_AXES 8

void * target_reporter( void * arg);

int emergencystop = 0;

int speed = 0;

int sunspeed = 0;

Vector3 wincha;
Vector3 winchb;

Vector3 suntarget;
Vector3 sunposition;
Vector3 sunstarting;

lo_address t;
lo_address ipad;

int axis_position[NUM_AXES] = { 1 };
int axis_status[NUM_AXES] = { 0 };
int axis_error[NUM_AXES] = { 0 };
int axis_number[NUM_AXES] = { 0 };
int axis_load[NUM_AXES] = { 0 };

int sunaxis_target[2] = { 0 };

lo_address local_broadcast;

int position = 0;
int target = 0;

int min = 2215;
int axis_target = 3686;

int max = 9164;

int amin = 28171;
int bmin = 27450;

int amax = 37077;
int bmax = 36949;

int aoffset = 2974;
int boffset = 3102;

int sn = 0;

int updatecount = 0;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int suntarget_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int min_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int main() {
	t = lo_address_new("192.168.2.57", "25666");
	ipad = lo_address_new("192.168.2.69", "1025");

	wincha.x = 0;
	wincha.y = 0;
	wincha.z = 0;

	winchb.x = 10972;
	winchb.y = 0;
	winchb.z = 0;

	sunposition.x = 5000;
	sunposition.y = 5000;
	sunposition.z = 0;

	suntarget.x = 5000;
	suntarget.y = 5000;
	suntarget.z = 0;

	sunstarting.x = 5000;
	sunstarting.y = 5000;
	sunstarting.z = 0;



	local_broadcast = lo_address_new("192.168.2.89", LOCAL_PORT);

	/* start a new server on port LISTEN_PORT */
	lo_server_thread st = lo_server_thread_new(LISTEN_PORT, error);

	/* add function to catch UPDATE_PATH OSC commands */
	lo_server_thread_add_method(st, UPDATE_PATH, NULL, update_handler, NULL);

	lo_server_thread_add_method(st, INFINITY_COMMAND, "ii", target_handler, NULL);

	lo_server_thread_add_method(st, "/infinity/suntarget", "fffi", suntarget_handler, NULL);

	lo_server_thread_add_method(st, "/stop", NULL, emergency_handler, NULL);

	lo_server_thread_add_method(st, "/state", "i", state_handler, NULL);

	lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

    lo_server_thread_add_method(st, "/min", "i", min_handler, NULL);

	lo_server_thread_start(st);

	while (1) {
		//Polling Loop
		usleep(1000000);

		int i;
		if(updatecount == 0){
			printf("NO UPDATES!\n");
		}
		else{
			for(i = 0; i < NUM_AXES; i++){
				printf("Axis: %d Pos: %d Load: %d Status: %d Error: %d\n", axis_number[i], axis_position[i], axis_load[i], axis_status[i], axis_error[i]);
      			}
			printf("Position: %d (%d percent) Target: %d Speed: %d\n", position, (int)((double)position/((double)max-(double)min)*100.0), target, (int)(((double)speed/100.0)*10));
			printf("Sun Position: %f %f %f\n", sunposition.x, sunposition.y, sunposition.z); 
			printf("Sun Target: %f %f %f Speed: %d\n", suntarget.x, suntarget.y, suntarget.z, sunspeed);
		}
		updatecount = 0;
	}

	lo_server_thread_free(st);

	return 0;
}

void loadstate(int state){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "ii", sn, state);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/pfs/ur/0006/loadState", message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void startplayback(char* path){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "i", sn);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void sendtoposition(char* path, int axis, int position){
  //sn++;
  if(sn > 65535)
    sn = 0;
  lo_message message = lo_message_new();
  lo_message_add(message, "iii", sn, axis, position);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, path, message);
  lo_send_bundle(t, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
}

void setspeed(char* path, int speed){
    //sn++;
    if(sn > 65535)
      sn = 0;
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", sn, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, path, message);
    lo_send_bundle(t, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
}

void error(int num, const char *msg, const char *path)
{
	printf("liblo server error %d in path %s: %s\n", num, path, msg);
	fflush(stdout);
}

int update_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	int PLC = argv[0]->i;
	if(PLC != PLC_NUMBER)
		return 1;
	int i;
	for(i = 0; i < NUM_AXES; i++){
		axis_number[i] = argv[(i*5)+2]->i;
		axis_position[i] = argv[(i*5)+3]->i;
		axis_load[i] = argv[(i*5)+4]->i;
		axis_status[i] = argv[(i*5)+5]->i;
		axis_error[i] = argv[(i*5)+6]->i;
	}
	position = axis_position[4] - min;
	updatecount++;
	
	Vector3 A, B;
	
	sunposition.x = sunstarting.x;
	sunposition.y = sunstarting.y;
	sunposition.z = sunstarting.z;
	for( i = 0; i < LOOPS; i++){
		A = vecSub(sunposition, wincha);
		B = vecSub(sunposition, winchb);
		A = vecAdd(vecMult(A,((double)(amax-axis_position[1]+aoffset))/vecLength(A)), wincha);
		B = vecAdd(vecMult(B,((double)(bmax-axis_position[5]+boffset))/vecLength(B)), winchb);
		sunposition = vecAdd(A,B);
		sunposition = vecDiv(sunposition,2);
	}
	
	
	
	
	lo_send(local_broadcast, "/infinity/updatesun", "fff", sunposition.x, sunposition.y, sunposition.z);
	lo_send(local_broadcast, "/infinity/update", "i", position);
	lo_send(local_broadcast, "/diff", "i", min);
	if(axis_position[4] == min )
	  lo_send(ipad, "/min", "i", 1);
	else
	  lo_send(ipad, "/min", "i", 0);

	return 1;
}

int target_handler(const char *path, const char *types, lo_arg **argv, int argc,
		void *data, void *user_data){
	target = argv[0]->i;
	if(target > max-min)
		target = max-min;
	if(target < 0)
		target = 0;
	axis_target = target + min;
	speed = argv[1]->i;
	
	if(axis_target < min)
	  axis_target = min;
	if(axis_target > max)
	  axis_target = max;
	if(abs(target - position) < 50){
		return 1;
	}
	int speedout = (int)(((double)speed/100.0)*30);
	if(speedout < 0)
	  speedout = 0;
	if(speedout > 30)
	  speedout = 30;
	setspeed("/pfs/ur/0006/pb/1/setSpeed", speedout);
	sendtoposition("/pfs/ur/0006/pb/1/axis/0005/setposition", 5, axis_target);
	
	return 1;
}

int suntarget_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data)
{
	suntarget.x = argv[0]->f;
	suntarget.y = argv[1]->f;
	suntarget.z = argv[2]->f;
	sunspeed = argv[3]->i;

	int a = (int)vecLength(vecSub(wincha, suntarget));
	int b = (int)vecLength(vecSub(winchb, suntarget));
	a = a - aoffset;
	b = b - boffset;
	a = amax - a;
	b = bmax - b;
	sunaxis_target[0] = a;
	sunaxis_target[1] = b;

	double high = 0;
	double aspeed = 0;
	double bspeed = 0;
	a = abs(sunaxis_target[0] - axis_position[1]);
	b = abs(sunaxis_target[1] - axis_position[5]);
	if(a >= high)
		high = a;
	if(b > high)
		high = b;
	if(high > 0){
		aspeed = ((double)a/high)*100.0;
		bspeed = ((double)b/high)*100.0;
	}
	setspeed("/pfs/ur/0006/pb/2/setSpeed", (int)ceil((double)sunspeed * (aspeed/100.0)));
	setspeed("/pfs/ur/0006/pb/3/setSpeed", (int)ceil((double)sunspeed * (bspeed/100.0)));
	sendtoposition("/pfs/ur/0006/pb/2/axis/0002/setposition", 2, sunaxis_target[0]);
	sendtoposition("/pfs/ur/0006/pb/3/axis/0006/setposition", 6, sunaxis_target[1]);

}


int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	printf("GOT!\n");
	while(1){
		printf("EMERGENCY STOP!\n");
		setspeed("/pfs/ur/0006/pb/1/setSpeed", 0);
		sendtoposition("/pfs/ur/0006/pb/1/axis/0005/setposition", 5, axis_position[4]);
		usleep(50000);
	}
}

int state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	printf("STATE LOADING here %d\n", argv[0]->i/100);
	loadstate(argv[0]->i);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
	if(argv[0]->i<1 || argv[0]->i > 6)
		return 0;
	char str[100];
	printf("PLAYBACK %d\n",argv[0]->i);
	sprintf(str, "/pfs/ur/0006/pb/%d/start", argv[0]->i);
	startplayback(str);
}

int min_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
//  min = axis_position[4];
}

